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(54) WALKING POSTURE GENERATION DEVICE FOR LEG TYPE MOVING ROBOT 

(57)Abstract: 

PROBLEM TO BE SOLVED: To smooth the vertical 
movement of an upper body and movement of joints by 
providing an upper body vertical target position deciding 
means which decides the vertical target position of the 
upper body based on the upper body locus and a leg part 
link locus. 

SOLUTION: A walking way of the feet is decided so as 
to be stored in a ROM 84 of a control unit 26 as a foot 
flat locus parameter. An upper body locus parameter is 
so decided as to satisfy a decided target ZMP and 
stored in the ROM 84. The stored parameters are read 
out one after another so as to be inputted in a foot flat 
locus calculation algorithm and an upper body locus 
calculation algorithm so that a leg part link locus 
containing at least either one of the target position or 
the posture in the tip part of the leg link, namely, the 
foot flat position and posture of the both legs and the 
upper body posture and a horizontal position are 
calculated. 
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5 2L<fcOfea51g*?S^- 3 r/h3K HJtj. ±tt* 



* Z b ^S^fCiSS-rtKf . SC2a. S2b. ^3~C 
5fc«> 0 sup i 0 swq -WfitK^SGSriSST * 5*5?? 

[0 05 2 ] 0sup >$*>-?0swq >S • • • 5$9 

±#nsz z b ^is^jcig^-rntf. sc 1 o #«5&£<*n 



q(Xb,Zb,Xsup,Zsup,Xb,Zb,Xswq,Zswq)< 1/sin (S) + l/sin(S) • 1 0 



[0 05 3] C©<t*©±{*i£S*>6&*{C±{*iiS<** 

£. ctnt. ^sc^ps-r-sffsoc^ur. 5£8b 
*r«srs±«:iS5 z b#?s^-rsci*sift-r*. 

Vsin( (5) + l/sin( <C <aftBg* • • -Kl 1 
Vsin( +l/sinC5-) SrCOifcWJ*. 14f?2##e> 

[005 5] urns 

5£8 a £/<c&33£8 b *m&?Z±#m2 Z b «C£/JnS 

< < & o (® < & o ) . c < -rtitj* 

3Zb(»2 ©J£fc&ftj#&©±#ie 5 KMMr z. * 
[0 05 6] ttM4 

sstc&o-ct^sfce.^ -#©s»©&i&£#*>±tf-5 

^©#^±tff±*#:£)WK#m<**i.5©-c. «©JI{* 
mfr'pu^xmts. se^r. ^fB#©Bgisft3is*e^fii 

fc/h£<#£. 30 

[0057] ±stemm± L/-c. c©^g©s6^=&t»w 

[0 05 8] ia4<CjrcTJn<. C©^SB, J£^*W»^t 

i"©^f©!}$ttfi*S9i«)6n. ^•tUCS^U-CJS©^ 

(ufeU/ciM©^-? F2 6©ROM8 4«ctSiWSn-2.. 
[0 05 9] H^L«cir>tf:79-f>«:*«,»TB* 

ZMP©fiSl:Ai^$n. ^©»S?hfci«ZMP* 40 

f(0 sup, 6 swq) = l/sin(0 sup) + l/sin(0 swq) 



•5. ffi. ZMP (Zero Moment Point ) «. ^SKJ: 

r^-r sisis^ia*©^©^^^®^^^ 

9. 1S1±;fr£a;b©^*Jfcffi±©*S{m£* 

[0060] ts^$n-ci>-5^7^-5f«^^ai3 
nts^DiBWHiTJw; XAfc^e^ttttEfritttbT * 

©atg&afc<fco*^©4>&< t fct^nA^^tfpse 

S^MgBl8, 20R (L) ©tfcg-;£i$) fc<fctf±# 

[006 1 ] JzVMtotfiicit. 057CI- • ft-KC 
^•TiB<. S 1 Ofrhmhhtd®.. S 1 2 Kj^-CM* 

B^raAt owswajw. mtiioms) triwdf-i'vid 

t £A t AfWJOrb. S 1 6 UZ'MAj-C&fM t <9i£ffi* 
SiJ) {Cfctt5Sm:<**¥tig (Xb.Yb). a«±ffcs£& 
C0xb, Syb, 0 2b) . WJ8©@<SU5¥ (SlTKIgS) & 
g (Xsup,Ysup.Zsup,Xswq,Ys»W),Zswq)*iJ:i>*^^ (0xs 
up, dysup, flzsup, 0xswq, 0vswq, 6 zswq)£rg£fct} 

-r£. ctt6©ffittBSB$fiir*-£. 

[006 2] >Xt>-C S 1 8 CC^rgg±^JSS Z b £ 
[ 0 0 6 3 ] m 6 «^©a«±(*»3^T <) XA 

[ o o 6 4 ] iss.. ia 6 Ks%rr*=fy XA«mr^©s* 

Mffi«r3^7C«:a:SSL//c*>©r*S„ B&&©tt«IS<!: 
©>!*)^4^-r-5i, sr. Mg5ft«:H-rS»«IK^f 



SCI 2 



[0 06 5] ±i*i«?*^-r^ft:*©^^<!:L/ 
rt<T©SC (^©2^7C-C©lttHJ©iC8 a{Ctl^) *a 

f(0 sup, 0swq)=C---Stl3 
[0 06 6] C©HJfi©0S§r«. SC 1 2 . 1 3 «M 



50 



[006 7 ] WT. 06©7D-ft- h^SftBJ-r* 
i. 5fe-TS 1 0 0«c*$t»rBS±ttiS3Zb*{S^tf>-r 
6. CWJ, aS±^iS3Zb*jR^^»{cJ:or** 
5fc*©tt»§fflr*s„ 

[006 8 ] i&H>r S 1 0 2 «rig-C S 1 0 4 Kit*. f5 

§EL//c@«±^*^g (xb, Yb) . temm±#mzz 

b BSJi»^ (0xb, 0yb, 0zb) . W»C^t,it 



(J) 

11 

©§tijE¥ (smwss) &m • mm (xsu P , ysup, zsu 

p, 0 xsup, 6 vsup, 0 zsup, Xswq, Yswg, Zswq, 6 xsw 

«> s e»{c3miu©gg{jsM©tis (mm t 

BtgJEE^ftS (S#Kfi5BE«) ©M ( TAxsup, AYsu 
Pl AZsupj tK^) tmm<OEtef&MBQ.B. (£&> 
ia«S¥14S <£tr&15^§) <om ( rAxswq, Ays 
wq, AZswqJ 5r*Jf>S. 

[0069] s i o 6 tcjt^. =Jj0g«©5Egtc 10 
s-^^ri27]R(D^{cae-5T^Fffliiswigsft0sup turn 

1&MffiPie9Ng gUTS 1 0 8lC'Mfr. ZfR 

1 3 ©Mi22©<B©gErr £5jcJt>£. ^faJCtt. @^© 

[0 070] fe^TS 1 1 Otcil*. Err ©iteSfffiflSiR 

+^/h3^^s^*«»r-r*. s 1 1 o-c&®uvw. 

BP*>> +^/h3«-i<i:«»rSn5i*«S 1 1 2«cii#. 
•€-©i*©±»K3Zb*B*±fln«S«:gMei/r^a 20 

[007 1 ] fife*. S 1 1 0-C5^Sn-&i#(iS 1 1 

C©5£«MErr ©5$£ Z b •cmVkftLX&tt fc©"C* 

- S - Err £JjD*fcffi> BP^. MErr Icm&SZmttc 

mzzb&zm&b tcmzmtc tc±#m $ mm tut 

ZbtCftAU S104Klg?'5. 

[ o o 7 2 i Mm<D?mfcm*%n,*®.wx'Kmm± 

*HS3«:-S • Err *JDilfc«3WBS»©S««*a*4 30 

[0073] @57 a- - ?+-htcKS£. tfto-cs 

2 0 K^"CB«±»*Tfi« (Xb.Yb ) . Sti±<*K 
3Zb. S&±ffcs£& (0xb, 0yb, 0zb> . Wlti!©g& 
(JSlrragS) {4S • (Xsup, Ysup, Zsup, 6xs 
up, ©ysup, ©zsup, Xswq.Yswq, Zswq, 6 xswq, 0ysw 
Q, ©zswq) a>6&*0©i£**vfw ^-XSUKCfcoT 
flPfc. WS10R<L). 12R (L) 
fci'l 2<i©Mg0©SSftS**J?>S. ^Jo/tKfflS^tt 40 
H5I2 L-fcR AM 7 4 CC«|frr 4. 
[0074] S 2 2 Kit*. gtabfcgmSA i 

&£J:-5K:. UiaLfcS8»«B2{c*jt»-cn3j<y h©£ 

tt&*irc&s©-c. mwmm-rz. m«ic«=i 

©S&&t*ira©Hit*Wt/%b»©r. 

[0075] c <Dmm<ommt±iz<DHa < m^tc<o 50 



1 0-86 08 0 
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r. MSjGFA (ft##fi) ©±<*WS*l*<'<^y-* 
#t5*.6*iS£*. ±{*©±T«i*J<fcy f Mt5©l»#* J tt 
6*T. j!S*iJ:0-»nag©«itI*vjN3 < & 

[007 6] Htc. flBBOfcJ: 5 fcgflff K 'J TJU£ -f 

[007 7] fgcc. «^m8^K#0T*t&5£&& 
#MU fl6*©J«l«r#SRS-a-4Ci*J-C* t W5fci&© 

m*imLx&ftm<DM&n&mmtm't>fc2 < t*c 

[007 8] H(C. ±(*©ISia*|fiJfiiH^# < ttSK 
cMSa*i6ifiUa3WS^:«:jS ofc t ^^PSA <b * S c t *» 

Ai (*t*> TSfe©r*n«m>. 

[OO79108BC (Dmwom 2 ©*Jg©JK«{C^S 
[0080] JV 2 ©HUtKWBtt-Ctt. 0 8 {C^"T*n< . 

m 1 ©His©^s§©±*iS3^r^=f xak^^ 6 

ttS£¥«iB*©-§PCC . (SiS©S^i)iB):*#^ 5 J: 5 tc 
b/c„ J:«Pfti*W{C«01 0(c^TJ:9(c. J£¥ (jSJW 
JE¥> ©SS&g©«i»*, *a#»y 

[ 0 0 8 1 ] m 9 itm 2 ©JU6©^!!§«C^^J|g©S!) 

ABS 1 ©SUS©JfJSS©0 5 <!: RfifCft^. 
[0 0 8 2 ] aT»Wr«4. S2 0 0{C*Jlir<ESjS 

CCS-^C»r#ai3n-5. 8StvcS2 0 2tCi^Ta«± 
ftiS3Z b*(59iJ!C)L. S2 0 4*igTS2 0 6tClA 

r» l ©JI*4i©^«ill«{cffi«±©fiiS©^**«> 

[008 3] WT. S 2 0 8&C»L2 1 8*ig-rmi © 
HSS©?gSS©ia6 7a - • ^ + -riR««c«HS%ff 

©HJ6©^sg©«ia«. sri©^jfi©jgas©*ti<ts«: 

[0 084] S2©HSg©^Sg«±IB©ia< «fiRL/c© 

r. *i©SUls©Bfi8il5«{c. ±fmd«Ht<^9> 



(3) 

13 

< *> ic±.#m 3 zmiEic&n-r zzttfi-czzt 
sec. ^tftfKyr^a'^AtciftiS^^surfSgts 

[008 5]^ IS 1 ©HJ&CDJ&JsgtCi&^.S <!:. S^fC 

l«:^-r<J:^«:. ±i*©iiS3£:£Jai*Kc^{t3-ti*C t 10 
[008 6] ±.fr<D±.Tm#i**>*>'Mhi£tc\.* 

©T^ilK**** < T£¥2 2 R ( L) ©««&0E#<a 

01 3ttf&3©iyg©JI^{C<&£ggT&6nS±<*«l 
8*©t&iB0-e&*„ 

[008 8] JS3©*56©0Si«:S&2 ©*Jfe©?fcgS©^ 

scwotc-^r©. £«. &immK£<Dm7<Dimm 

[008 9] 01 3«-en«C<fco-Ofc£3*lS±{*^3 

fii-c**). fifl^<B&B«iUGttc<i: s^©in 
< . mm&mcm>^m.-n^ s *rif * c t tc j: 9 . 
±#i«3 *><s< -r * c i*«-c# 4. 

[0090] Hw©2 JSiMf 0#» h 1 O^fftf 

4fcs>&{£ <■*-•£>£. jsswgpi 6R (D *ft*fciMrr 

35fv S£*tiI*SKjB#S<!:> 1£»OJ:W*»<4-j 40 

wrr*. 

[009 1 ] SH3©5yfc©05§«±iE©$n< tJJSc LfcC 

T3!lliaflE*fii«S-a-fciMf**!ir*C4jW-C**. 
[ 0 0 9 2 ] H 1 4 «C©^K©»4 ©HJ6©0!&fC<fc 

[0093] *4O30fc4>JBjB«>. #3®Xtt<Dj£tt& . 
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<. 3fi©S¥8l&>^^ 5 {CO/c. BP 
[0094] <SSUE¥IU»»aiTJb=f 'J XA 2 r|?tti 3 

<4S • mm) tt. IS l ©«lKD#»T»^fc:*3feWa:« 
igfirc**. fl6* v <s;BS¥«i!»»ffiT^=ryXAir 
#HB3*i4GEKJS¥ftS • KftttWMIJ: 0 

xa 3t?»tb3tis<5ffifii«sm<ii«fc o fcii»!)cc^s&-r 

[0 09 5] RNUC. ±ftiK39t£7'Jt'=r»X.&6 3ffl 

ffl.^3n. ±#iSteT;u=*yXA2-cfcS3*i,*sru© 
nm<m»vm^itmmtfjtta2ta ec»u ±1*1153 r 

;WJXA1, 3-C9W£3*l-Stl2bl,3 tt. _t#iS3© 

[0 096] ^3ft/c3iI©ffifcm>T¥iJjXci * * 
(CA^73n. -ec-C^t*6¥^lfilZbav*i#m3n^. 0 

6 nfc±^ias 3 4s <fc i^-eo<6©/< 9 y - £ 1» 

W3tiS. 

[0 097 ] »4©H»©^.««±Sa©iP< m&LUtc<D 
^©in < . 3 <!©&»l5©¥i$J*iZbav*;fcat> 5Ci "CMiK 

jg«:*tL,r®*i*/jN3 < r*c i36«r*. tiMRtt<c±tt 
p^-r * 4 * a: i'«©)if#©A# << >#ff 9 pscc 4> . ± 

[0 09 8] CCt, Sr2ai/>HS4©HJ6©JgS8rffl 

b ) ^WS¥©t)iSJ*. *WBaA*^{cSlSL/c7^;l/ 
t> (#g§^5 - 3 2 4 1 1 5-SfieiE©e3ti) *Il»T4 

[009 9] gfc. »2&l>LSr4©SQ6©»S8(Cte«,» 

r. ±»©a«s^ • B«*¥&g. s¥©ss«i§© 
©^*«TK7F-r. 

a ) B«±«sMP*M*#i. W-ff- ^-CIBSS 
nrt>-5«^«:K. f J -3'f i -^l'©3^*«r-r-5. BP 
3g«E^J t J: 0 *>*4^lffl5fe©a«±#fl[g*S* 

[0 100] 
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m 



Xbv(t>=£WnXb(t+kAO 
k=n 



Xb(t)l*l5J^G>BS.Lf**¥tt». 
Xbv(t) li^«|t<Z>«S±«:*^tea„ 

C o i o i ] b ) ^©a^i^Tk^fiig^^ a )v$ic 
air. ^©etrasst* 

(TnS +1)/ (TdS +1 ) 

tC&Jg* Si. Tn>TcHC-rtx«. 7^;U^«rilL//c±f* 
»a»ra:7c©*M*J: 9 fcfSfBjetycftfrT*. 
CO 1 0 2 ] m 1 5 «C<D^©^5 <D|£SS©ff5SSCCtfc 

zgi&omi&zmTm.m-fa v ?mx$> <o . 0 1 6 «^ 

©B&fF. IP%. ±<*it5 3 '^T ^ y X A ^ 7 □ - 

[0103] 3&5©*&©0<8rc». 0 1 5 (C^-Tin 
< . ^ l ©ifete©^^^^ US 4 aymmmmicj: ~> x 
&ibtc±&lK$$:7 (n-;«7<JW) (can? 

[ 0 l 0 4 ] «T. ^5CD*M©^fig«:^i±f*]S39l 

Bj-rs. ft. &®£.f$,icM-rz>* -{>T}i>JvzMzm 

[0105] trFi&HJf-S i > S 3 0 0 fC*jt,>-C!& 1 © 
^©^r&^c±#igi*S«£;l>--^:> (067P- 
+ &H?fU ±ftiS3Zb*3}?ii«)-5. ft. ± 
<*iS5§«SiJ*B#HIA t cr<k(CStta$nS*i. ZtlZ&tf 
©1 mMlctifc-oXTf;? t > SfcCCEll 1 CCtfU/cJ:^ 
(cct ruiS^j <fc<->5) iLra^sciAi-c 

[0106] i&t>TS 3 0 2t,cmA,X&Sbtc±.#m2t l C 

[0107] 3l5CD!£&CD^8§»±ie©Aij< ^L//cC 
ir. ^©^©^©^K^T^ffittasaccJ: 
9±#©±Tfai££t&fM£-Jl/h;* <t4C 

[0 108]«, C©<fc 5 Jr^T. rassft© 

[0 1 0 9] HI 7 rac©#SBJJ©S&6 ©SgJS©0fi8{t& 
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2>mm<Dmf&*9ik-r®i&-7a v zmx*> <o . 0i8i« 

©»fK bp*>. ±(WS»Sr*rf';XAtjW7n- 
[Olio] fif6©jW£©ftJ*g«:*o-ct;t. un©HJS 

[o 1 1 1 ] 0 1 8 7a- • ? + -v*g-muxm6<D 
io !^©0&K{fcs§SE©±#iS3^7Ji/:J'JXA*i& 
w?z>. ft. ^g^ccH-rs^^>TJU=fjXA«^ 
i ©5as©jf?sg©ii 5 tmmx$> s. 

[0112] S 400 (CfcOTlfr 1 ©H«5©?fc«£-e 

m 6 (cm l r m^tc^m^ir o r 3waE«aBi-c<Dea±# 
(^HifiB) £f#£. sh>t:s4 0 2tciiA/c > $fc 
tcss i (Dist&mm&t bx&^tcmno&mttm^ 

[0113] m^xs4 o AK.mA.xmmm (b##j 
20 t) rcom^wi. w2*^-r-2.. fib. iwi, 

W2«. Wl +W2 = 1£«JEU Wl, W2-€-ti-? t 4a 

oeicm^xmrn^m^nm-r z>. in^. mi. s&2 

[0114] ®6©HJ6©^SSCC*^>-C«±fecOin<«l 

«±»i«sfc-^(,»-c. m\m&&<,t%ivxwz.ia.m2<D 

30 {g«-r-5C£*i-C*.5„ fit. h o *m< tisr-si. 

s. 

[o 1 1 5 ] ft. mecDmmommximmmifizrxD 

m^LtcHi. 3<lJ«±{c4>ffi;SKpJti-C*5. H(C. SO 

[0 116] ^6©|^6S©0SS-C«. ^2Mjfi^ 

«. fifefe^^iS. il»{CB-^tf. SSl©J|J6©ffW«ci,> 

urns ©iiifio^ssfc^b/c^^Ji-c^^ffl^rff 

40 -5 J: 5 JC b ^ 1 ©HSfc©0&& t» 1.^5 ©3»S© 

^ss©(,»-rn^*^(,»rf#-c *>m^. 

[0117] mtf. ^1©HJ6©^. S»2©Hi6© 
je.®. *SC»«^3J5:^bm5©*te©^4fflt,»-Cii> 
i*) ira©±#ifi>3M&?££3fc«>. MK-e©^?: 

^*fcasK©±»iB;3M^*^Lr^tie»©ii^ 
[0118] mk, «T©#ffi{c«t-,r/ii!S^ffr*> 

a ) -mm m^micit. ^^{c^^nse^r* 

50 £) • 




(10) 

17 

b ) -ym/:': :rmmm (.KawicommvBi&x 

av-r f.* 

e ) F •? . : ?S& 1 ©S&S©J&S§*/8l<>-C 1 &9t<T>W. 
ft\,G>& h . («»J> U^t=~ * *jtt<ctt&tU U 
ML. DliiS("i!; s -:ct*»2^— ^<M6«e>*>6tt*lW-# 10 

ifiiff5?:4KT-, Fa) . 

[0 1 19: tf^OXtfcDJgft&Jittr). #£ettpg© 

tt©±T;jajin%fi«i/fcc»*B^K:«*s»tt^iar* 

C0120! 1-1 i 9«C©^©07©3liS©^S§CC^ 20 

^-rtagg^a ? t> ®x$> *) . s 2 0 «-e 

[0121] ^7©5esg©m®{c*or«. ^6©*Ji 
©«!£+ ut 5 ©fste©^£m>Tf#7cg«LL#ia;3 * 

[O122102O7D- - ? + -hi&mi>X&7<D 

^Ji©^JSgtc^-5±»iS3SMer-»u^ y XA&t»nrr 
•5. ft. ^s^ccM^5y^>T;ud-yxA«^ri© 
SQS©ffJS!©0 5 <t i§n$-e* s. 30 

[0 1 2 3 ] WTUiWrSi. S50 0SCOI/S50 6 

(c*$c >r ^ 6 <omm<DBi8it mmo&mztf ~,x sm± 
&m s zmc&. s 5 0 8 «cjtAr»/cffli(cm5 ©hjs 
©^s§<b mm©:? .* ;u zmn&n *> . 

[0124] ^7©^Jfe©^SI«±IB©*n< tftl£Ltc<D 
t&XZh. 

[0 1 2 5 ] 02 l«C©j%W©^8©Se6fe©»SS(C^ 
5^S©t8^€r*r^fi6^ta y i'iTS.S. 40 
[0126] ^8©*tt©m<ISK*or« > mm<Dto 

<. ^y©^©^.®©*^^^^^^^^:^^. 
bp*, jiii mmmtm2mm^^ti^Hmmw-mbt 

WimK.£.l&U. **l£S#W5. W6*fflloo))Dl 
*W1, W3. W5. *SC^JW2, W4. W6«> <e 

[0127] &&<»m.M<Djm< l ci$^xkt±&.<Dim< W so 
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[0 1 2 8 ] ft. Cft«±K¥?f{t*SC>«flnS ;i Pi$l£ 

moxi>. mmomm. mm&it, micwmTzmftv 
ihzfriixiibz. 

[ 0 1 2 9 ] m 2 2 «C©2felE©J»9 ©jSS6©0J£(Cfii 
•5^a©«JfiS**f UiWS-C^S., Sf9©HJ£©7ja8K: 
**1>T«:4JS (S) mTOVtfy h**fm£U ■?•©# 

[0130] JWTIttl§-rii. 0^©4S*tfa^f h 

ioo«, ±& (Sft) i o 2 i. *tiicm?mmm 1 
04 (ffrfaofc^i©^) *ftuxm£2tiz>4*<D 
wisp'; i o 6#>e>&£. 4*©wsi5';>i' i o 6« 
-e©5t^ (#P5feJS> i o 8 £©ratcj$Hiii5 1 

1 0 (miZUfcm2<DfflB) E^ttO&l* 
[0131] H^©4S^fdJl<y h 1 OOKiJWfc 

es«Jr©iiis©^ssr , ©2js^fa^f h 1 tmmic, ± 

#1 0 2©zMP(4S (Xb. Yb) tei^ (dxb, dy 
b. 0zb) meb. *X^x±to<Di52Zb&$cibXWffi 
104, 1 1 ozmmfflfflu, k-iXmiCttUX^fft 

[0 13 2)±IH-C. 02 3{C^Tifl<±{*Bf3Zb 

«. ±(* i o 2 ©s®©^'^ 102a tmm 1 1 2 t<0 

?l 0 6©KcB8g51 1 0©ftS*6n. P5feigl 0 8© 
fig* (Xn, Yn, Zn) (n : 1~4) Xfjkir. 

[0133] 09©^«fi©0js§«:^^^a©a6^*ia2 

4 7X2- • ? + -h*&mLX$SiBM-rz > t> S600* 

6.S604 £xmj<Dmm<mmtmm<D$mz'if ~>tc 

fa. S 6 0 6 ICMAsXmn t KfeW-SS&iiflwk^ffiK 
(Xb, Yb) . @«±«:^ (0xb, £>yb. 6> zb) . *J«fc 
Vm&MikimE. (Xn, Yn. Zn ) (n : 1~4) 
«>. S6 0 8{Cji^e«±«:^3Z bfcJfci&S. 
[ 0 1 3 4 ] 0 2 5 tt-e©f^l*^-9-^U-^> • 7 
a- • ? + -bX$>V. MTItlW?Z>t. S70 0. S 
7 0 2 4StS 7 0 4 tCjl*. g«±i**-spaS (Xb,Y 
b). ■fSB^itti^i* Z b . ±#^(exb, 0yb. 9z 
b) . *J«fc^J»l$feffl{aS (Xn.Yn, Zn) ^P?,. SW©S« 

m-fwm® i o 4©{4g (ffi«) tmmmftffii o 8© 

(4g©M (AXn, AYn, AZn) E12 3CCCI1 

6©ffi*^-r. 

[0135] -X^XS 706 (Cji^^*/cll*>6Sm 
©^*fflC^rKB9ISft0 n4#»*. »>-CS7 0 8K 
ilA-C^Err *0^©in< gtffl S 7 1 0 (CMA,xm 

«S 7 1 2tCjitr<!:*{C. SESttSiStiS 7 1 4tC 




(11) 

19 

[0 1 3 63 ISI9©*;fo©^Sg«±fS©in<flt»i£0fcC 

(2W) v t®mK.»mic±&nz*#ibz 
citf-css. ft. 4£#tTn#», Ko«*5*i,fcaJ. 

[0137] ±iB©in< . 'J>u < t i>±&2 4 <h. mz 10 

ittJClBl ©Mg0 1 0 . 12. MR (L) *^l/ta 

feznzm&frvwu vy 92 miewsB 

>2 ifi*<D9&%$te-eiC'J>rj:< £ & 1 <I©fH2©0gg5 1 
6R (L) ^urfc^Ws^Sfra** h©3*g£jS£ 

a*KK. ±<*«ig*£f^T*±#mi&i9:^I8 (S 1 

6) . m5m&'j>*imm>mm<iL&i!£V£m>'j> 
%<£& t>-rti*>*#tf«iS5 > sjusb 
■;>?iU8i8£¥f8 (s i 6) . fejrDfmrteiftjesn/c 20 

*©ft?*g^iB±f*©ifiiS;frfiSfii(ia*ifc 
m-?z>±msm.jmmmfiLa®:j£&&. (sis, s 1 0 

O&Ut/Sl 16, S200?tl>l/S2 18, S300 
&OCS3 0 2. S40 0fc(,>US4 0 6. S5 0 0& 
ULS508, S608. S700ttt,>lS7 1 6) * 

tmmfrtjKzmztfi. mf3miaj:vfm2<omm. a 30 

#ffJ&CttSB2©l«g!l 6 R (L) <D'J>tj;< th^Ttlifi 
[ 0 1 3 9 3 */c. ftIB* 1 *5£.V%2<DmB<D'Pt£ < 

firia02©Mg5 1 6 r ( 1 ) ©^{fcfoj^iig 
s«c-o*arHtriaa^M^©fii*!*ii«:^fr.siB<^fiX 4c 

htc. 

[ouuifc. KnaJi<*©*¥*isigsiaa*4v> 

(L) . <k«PS»WK:«iaifflIJSmMg5l 8. 2 OR 

(l> t£ t-immtKm'c&zto <&mL &. 

[0 142]*/:. mii±{mm.?>faBM<iLM8itt& 
tt. tfrfB±ft©ISit^l6l©eS{ag«:n<l (n^ 1 ) & 
St5n @Stag^#IS: (S4001&UI/S402, 
S 5 0 0 &(,>t, S 5 0 2 ) . *s<i:zmim£m&nm<D 50 




1 0-8608 0 

20 

@til{ia±©¥^ffi*##* WB#tH# © ( S 4 0 
4. S406, S504, S 5 0 6 ) 5j?S>fc¥ 

^ifiiK:a^^-rB?fE±^©Sfiitt^(6]B«fiia?:^-rs 

[0143]«fc, fJIB¥l^B*iftnS^ffl ( S 4 0 
4, S406. S504. S 5 0 6 ) r*£$n< mfii.l> 
tc. 

[ 0 1 4 4 3 !&!B±#$flS;£figt§!ftEfc5£*« 
tt. Ji>IiB«£#to;te¥i£fiI©4>ft< ±<b^?tifr%¥-tt 
it? ZW-mt&WL (7 S302, S508) * 

[0 1 453 **:. B«rfB±«^6ii:^aS{4S^*l2 

«. ^*&*it»«iRmjiisc©^< it>t»-rti*^ffl 

[0 1 4 6 3 MCC. m&®m2h,tc±.ffih8ft. 
g^T@MM0#£*»Sg«Wgi5ft»tiJ3M8 (S2 

o) . te£miiisimmm®n£t£Z£'5< l cmsmiteJ: 

^02©Kg5*^ffl©J®T-5^(4$iJai^e (S 2 2 ) £ 

[01473 sfc. OTismissgBba;** h&2m*mr 
zmf o * » h -c* o . Mian i <ommi>mmm 1 o . 

12. 1 4 R ( L) . w!iS.fS2 ©KSp#I&WS5 1 6 R 
[0 1 483 */c. mim3K&®>V#v Mfi3H&±<D 

nm^nt^mmiu-^v b -c& o . mrian 1 ©re© 
tfttwis/iarasi 104. s?fan2©Kias*sKKgis 1 1 o 
-c&ztaKmmutc. 

[01493 &.±. c<D&wzm i vmmmmfrtim 

t. mzfrftmmw&fcdswp.esHq) iir». sci 
St»fiiK:3ff-^< if(0sup,0svwj ) ©{t*5®jf>r^:* < 

[0 1 5 0 3ft. f=Ct*. 1/f = 1/C-^- f = - 
C «c i'5C^0-T -5 C i # £©T?. ±fB 1/ fcHiS©^ 

sg-c*tf tcmmmtM®. t <DRt> o . 1 / f - 1 *m 
[01513 mi<Dmi<Dmmvit, »6*^a 

©ffe©B8ii5«:Wr^MS:'r*o/cO. ^MiBtCK^'SM 
S"C&oTfcm>. -€-©J:^^ti^-Cfc. 0^©a«K^ 

ht?«^r©rag5ft«±«:©{sia • m&±m*&m<DiiL 

e icggi? 5Ci*5-C*4©T. m ©HJS©^SS©^ 



Ctf) 

21 

C 0 1 5 3 ] ±^»S*S^^(SJCC-Di>r^Jt)/c 
[0154] — ^T©W©B8t5S:*i6<a*ffljt.5a-1<5' h 

mmmicm-ri&^m^^^tn^ nam ft*- io 

*. 

c o i 5 5 ] maxm i ©KSg©?fjs§-r«s^r«>ofc 
*s««±-c**iw:. ^*S5j£-f*±#» <*©»?#*? 

[ 0 1 5 6 ) U#-j h©«ig#0^©«fc5K2E£*tf*"C 20 

M&ft<ow&t tit, 2E&wgpfc^-r 
tttfcsn. fit. »&4»(cse<b3««cc^. ±t*©± 

4 1 1 5#&*8fEIS©&a5) £AH>-C6Atr>. 
[0158] S/c. M^M£8©JlftSJBl^<!:©H 30 

©fln*¥*a*tfT*.tt\ ±{*©±Tflnjisai*i*>h3 < r 

5Ha#5» Y<D<g® <£l»g5ft©iSfi) *i#fiErSortl14 
[0 15 9] COftH^Jij&ffaiK? hfeJ:^ 

4je*ffpjK'» KCMi/TSiwi/rtr/cj&s. •eti&tcR 
6-T. 3J^SiHS5ja^O$ita>K» htctftflr** 

r±««3j «flffir*»6±#*r©aifflB»4jB*ftit 40 

[0160] Site. mftm*s<t:v/£tc\,m&-b>-v- 

[0161] 



1 0-8608 0 

22 

[0162] HKB*JE3tlfc±fl«S*^tf«IU»*3«E 

[13 1 ] C©H^{c^4WS5^ttn^i» hO^S&ftgS 
[02] EI 1 &C7jVr25*M?D#? hCCO(,»"C<^>- 
[H31H1 fC^-f$Sg©iMiSlJ.- » h©«ffl*^r ^ 

d s- i»0-c*&„ 

[04] t©|fcB£{cra*W5$£ttP#* h©#S£fi£§S 

[05] 04©«g©S&fP*iR-r^^> - 70--ft 

[06] 05O7B- • h©_t{*K Soffit* 

tkTI^T'-iU— ^> • 7 a— • + — H^?**. 

[07] 0 l*J«fci>'02«C7K-r2S^fP5Kf h*2^ 
tc*7vI/4 or^UfcSi^0-c*-5. 

[ 0 8 ] c omwom 2 ©njs©jgai{c^4i6«©«fiS 

[ m 9 ] 0 8 ©ssg©ftft©*©±#fi?;* awtf^fii*^ 

[010] 09©7P-^ + -hrffl^*««lJg^lKB 
[01 1 ] 01 0©<SffiifiWJS¥«lJ»(C»-rS±«:©«l 
[012] C©^©^3©HJfi©Ji5!!§{C^i^g©Kl 
[0 1 3 ] 0 1 2<DmSK£l&2tiZ±#m2>l:fjk-r $ 

[014] C©^BJ©04©HSS©^{C^5^S©« 
jSfcTjvr&^p * ?®-VihZ>. 

[015] C©^Bj<D05©*ifi©0Sg(C^S^g©« 
^^■TtSfig^n » *0-c**«, 

[0 1 6 ] 0 1 5©^S©S6f^©*©±^ilfS^)£^ 

[017] C©^<D^6©HJfi©ff^fC^.5^S©^ 

[0 1 8 ] 0 1 7©i!H©t&ft©*'©±#iif;*&5£ftl8l 

[019] C©^Bj©S7©HiS©0SliC^.5«g©« 

[02 0 ] 01 9©^g©SW^©*©±«:iif3^^ 
*Tht^)\^~^> • 70- • ^-f- 
[02 1 ] C©^BJ©^8©|liS©0!g{C^SSg©t8 

[02 2] C©^Bj©09©Hifi©fi5SS(c^^gr. 



(13) 



23 

[02 3] 02 2<3D^ScD*0/>^y-^CD5ES4^T 

[02 5 ] 02 4©^aol6f^O*0±i*«$^^ 
[f?-SKD»9J] 

2 

10, 12. 1 4R, L 
1 6 R. L 



IV>9 



10 



* 1 8, 2 OR. 
22R. L 
24 
26 
1 0 0 

1 0 2 
1 0 4 
1 0 6 
1 0 8 
1 1 0 



ftmW- 1 0-8608 0 
24 

±# 

4JiiMfo#?F h (W^Sfr 



[0i ] 



[02] 




t)y<?90(±*24) 




[010] 



3<7 



«F§a¥ 1 0-86 08 0 



[03] 



26 



,82 



ROM 



RAM 



74 

r- 



70 



A/D 



86 



60 



~44 



x > □ — ^ 



i 



1 1] 




WW 



[05] 



(en; 



s/0 

2 



00 



i/ 



S72 



t = t + A t 



S/6 



BWStcait*a«±#* J HM(Xb, YbX g^±ft£ 
»fca*fi¥tt« 



gjgLt#&£Zb£*#>§ 



S20 



§«±#?K¥ttS ( X b, Yb)> S«±ft»arZb. 



.S22 



(RETURN) 



C15) 



14] 




to <i 
4C MS 



a i 




-H 



M 
CD 

>» 
co 

X 
CO 




I 

IP* 

— s. 

m 

5 



IK 
tt6 



O 



(16) *JP8¥ 1 0-8608 0 



[06] 



(entry) 



S100 



S102 
1 



S104 



£&£?! ^ Ysud . a Z sup ) t jffiWcD B^flSM 

ffittgt g^TttS©^ ( A Xswg. A Y SW g t A Zswg ) *. 



0sup=aeos(((AXsup»AXsup+AYsuP*AYsup+AZsup*AZsup) 
/C2*L*L))-1| 

0swg=acos j(( Aj(swg*AXswg+ AYswg*AYswg+AZswg*AZswg) 



#5£<fc U Mfl&ft CB8 <T ©W>2©<I©g Err £ § 



(return) 



^ Yes 


EXIT A fSU2 5108 

r 


lErri tm®m / 

1 \N0 


TIB©®g5£lZ<fc U*« S 5 






Zb=Zb-S*Err 



s= 



cos( 9sup)» aZsup 



L*L 



=^ — ^ + cos(gswg)* AZswg 

( 1 - cos(0sup)*cos{0sup)) *sin(0sup ) ( 1 - cos(0swg)*cos(0swg)) * sin(0swg) 



(17) 



10-86 0 8 0 




112] 



±*** Zb 



13] 



123] 




[HI 4] 



feffffifi 



fUMZtt 



. 1 



2 



. c- 



3 



Zbl 



Zb2 



Zb3 



Zbav= 



Zbt+Zh2+Zb5 



Zbav 



1 0-8608 0 



■KJ <] 



IS 




3B 

12 



CO CO 



IS 

-H 



Si 



IXl 
CO 



X 
Q> 



■m ^ o 



4$ 



I 



t7\ 

H-irs 



I 



o 



(19) #PfJ¥ 1 0-86 08 0 



[09] 



(entry) 



S200 



■S202 



S204 

L 



OO 



S206 



%&W<DBfflMmBmt ggJE^ffiffi©!! ( a X sup. 

Ysup, A Z sup .) £mogflQRHtfAfil£:fiaR£? 
ttft0i(AXswg, AYswg. AZswg)£##£ 



0sup=flcosl((AXsup*AXsupfAYsuptAYsup+AZsup*AZsup) 
/(2*L*L))-t| 

0swg=acos{((^swg^AXsw^+AYswg*AYswgf AZswg*AZswg) 



#5££ UNffift CS8^ $£5£(DM2l<Dffia>g Err£:£0> § 
ET=l/ s in(Osup)+l/sin (0swg ) -C 



Yes 



I Err I MMI 



EXIT A ./ 



No 



(RETURN) 



S216 




(20) $#53^ 10-8608 0 



[015] 



Xsu>. Yap. Zap. Sxsup, fry sip, 9isv 
Xsva. Yswg. Zsw3. wsn #VWS. «B"3 



Xb, Yb, Gxb, $yt>, 0zb 



ft*?* 



Zb 



(LPF) 



ft* 



16] 



C ENTRY ) 



S302 



( RETURN ) 



C21) 



10-86080 



17] 



% 2 warn 

(SMS) 



=W 1 *«i huh* 



S*W 1, W2 



[018] 



C ENTRY ) 



S400 



l*Jfc#KS=w i *f 1 f gf-tf J * 
+ w 2 * £ 2Maa^±frsss 



C RETURN 3 



(22) 
[01 9] 



to 
















B-nn 








































8**> 


































n 













(23) 



&m¥- 1 0-86080 



[02 0 ] 



C ENTRY ) 



S500 



S502 



g«±#^$=w 1**1 mm±$Mz 



C RETURN ) 



S506 



Y 



S508 



!22] 



-fcfMMOa. Yb, 2W 




(24) 



Itm 1 1 0-86 08 0 



m 1 



[02 1 ] 



n\mm 



S2ISOT 



+W2; 



-W2**2®XU 



MAWI, W2 



47;nfu;te 



*#W3, W4 



+W6**6*» 



fi*H£7JHfV;a 



S#W5, W6 



(25) 



ftffl 5 ?- 1 0-86080 



CS24] 



(entry) 



S600 

L 



1/ 



t = t + A t 



Y/S604 



S602 



S6Q6 



B«J:#*fcZ.b4*ftS 



_Z1 



SS/0 



gg±tok¥ttfi ( X b, Yb). -aSLtfmszb, ±#» 



(RETURN) 



(26) 



#§fj¥ 1 0-86080 



[12 5 ] 



Gentry 



S700 



/ 



S702 

1 



00 



S704 



«)^gtta©M( AXn, a Yn, AZn 



0n= acos {(a^j* jxn+A Yn*AYn+^Zn*AZn) 



*5t«fc U#5tog E r r $ § 

4 



^S708 



Err= I 



l 



n=i sin ( 6 n ) 



-C 



S770 



Yes 



I Err | mnm 

mi 



No 



EX I 



S/74 



Zb=Zb-S*Err 



C RET 



URN) 



S = 



L* L 



cosf dn )* AZn 



n=i( 1~cos(0n )*cos (0n ) )* S i n (0n ) 



nmw-i o 



-86080 



[ £^rs5?ij ] mtmm n?k<D2 (ommK. <t zmjEvmm 

[aPHE»] lf52g|5f"3JB3K# 

[^fH3 TORI 5*8^260 (2 0 0 3. 8. 2 6) 



C<Zr§3#^3 1 0-86080 

[•St»JH ] TOJc 1 03M.8 7 B (1 9 9 8. 4. 7) 

[*PSMH*] 10-861 

imm&m *»F9-2 13952 

[fflfig#f*#glJ&7JiS] 
B25J 5/00 



B62D S7/032 
[F I ] 

B25J 5/00 C 
E 

B62D 57/02 E 



[ffittiB] TORI 5*£6£2B (2003. 6. 2) 

i&msiE i ] 
[ffliE*fm#ffl«] Him 
imjEttm.ma%) 0049 

[0 049] q(Xb,2b,Xsup,Zsup,Xb,Zb,Xswq,Zswg) * 
S«,»«f(0sup,©s<« l ){CC<D<fc5&ttH*t*4Ci*8g 
* it T . &<DgfcSI*SSa>*i * C <t 3 . 

[**&ffiiE2J 

[*iE****»s] mmm 

[»jE»*«B«] 0 128 



[0128]^, CftJ^«C^fl:**l>t3ftlS¥$£ 
[«RMiE3] 

iWEMmmmz] 0129 

[01291022 IZCVmXOfBQ <Dm&V&miC% 



-» 1- 



